/*
@brief: 弹道解算 适配陈君的rm_vision
@author: CodeAlan  华南师大Vanguard战队
*/
// 近点只考虑水平方向的空气阻力

#include <math.h>
#include <stdio.h>
#include "user_lib.h"
#include "SolveTrajectory.h"

// struct SolveTrajectoryParams st;

struct tar_pos tar_position[4]; // 最多只有四块装甲板
float t = 0.5f;                 // 飞行时间

/**
 * @brief 单方向空气阻力弹道模型
 * 该函数用于计算在单向空气阻力作用下，物体在特定速度和角度下达到特定距离时的高度
 *
 * @param s:m 距离
 * @param v:m/s 速度
 * @param angle:rad 角度
 * @return z:m 返回物体在该条件下达到的高度
 *
 * 此函数基于单向空气阻力模型，该模型考虑空气阻力仅在水平方向上影响物体的运动
 * 通过计算物体在给定速度和角度下达到指定水平距离所需的时间，进而计算物体的高度
 * 如果计算出的时间为负，表明目标点超出物体的最大射程，此时函数返回0
 */
float monoDirectionalAirResistanceModel(struct SolveTrajectoryParams *st, float s, float v, float angle)
{
    float z = 0;

    if (v == 0 || fabs(cos(angle)) < 1e-6 || s < 0)
    {
        t = 0;
        return 0;
    }
    // t为给定v与angle时的飞行时间
    t = (float)((exp(st->k * s) - 1) / (st->k * v * cos(angle)));
    if (t < 0)
    {
        // 目标点超出最大射程
        t = 0;
        return 0;
    }
    // z为给定v与angle时的高度
    z = v * sinf(angle) * t - GRAVITY_S * t * t / 2.0f;
    return z;
}

/**
 * @brief pitch轴解算 计算俯仰角补偿
 *
 * 该函数用于计算在空气阻力影响下的目标点所需的俯仰角补偿。
 * 它使用迭代方法来近似解，考虑了空气阻力对轨迹的影响。
 *
 * @param s:m 目标距离。
 * @param z:m 目标高度。
 * @param v:m/s 子弹速度。
 * @return angle_pitch:rad 计算得到的俯仰角补偿。
 *
 * 函数使用 atan2 函数计算俯仰角，然后使用一个考虑空气阻力影响的模型来计算实际高度。
 * 如果计算得到的实际高度为 0，则表示无法继续计算，俯仰角被设置为 0。
 * 函数中包含了一些被注释掉的代码，这些代码可以用于微调迭代过程，例如根据预期高度与实际高度的差异调整临时高度，
 * 或者根据该差异的大小提前终止迭代。
 */
float pitchTrajectoryCompensation(struct SolveTrajectoryParams *st, float s, float z, float v)
{

    float z_temp = 0, z_actual = 0, dz = 0;
    float angle_pitch = 0;
    int i = 0;
    z_temp = z;
    // 迭代过程
    for (i = 0; i < 20; i++)
    {
        // 计算俯角
        if (s <= 0 || v <= 0)
        {
            return 0;
        }
        angle_pitch = atan2(z_temp, s); // rad

        z_actual = monoDirectionalAirResistanceModel(st, s, v, angle_pitch);
        if (!z_actual || isnan(z_actual)) // 如果超出射程就归零
        {
            angle_pitch = 0;
            break;
        }
        // 计算目标高度和实际高度的误差
        dz = 0.3f * (z - z_actual); // 0.3
        // 修正高度架上误差
        z_temp = z_temp + dz;
        // 判断误差是否小于阈值，小于阈值则结束迭代
        if (fabsf(dz) < 0.00001f)
        {
            break;
        }
    }
    return angle_pitch;
}

/*
@brief 根据最优决策得出被击打装甲板 自动解算弹道
@param pitch:rad  传出pitch
@param yaw:rad    传出yaw
@param aim_x:传出aim_x  打击目标的x
@param aim_y:传出aim_y  打击目标的y
@param aim_z:传出aim_z  打击目标的z
*/
void autoSolveTrajectory(struct SolveTrajectoryParams *st, float *pitch, float *yaw, float *aim_x, float *aim_y, float *aim_z)
{

    // 延迟时间 = 偏置时间 + 飞行时间
    float timeDelay = st->bias_time / 1000.0 + t;

    // 预测子弹打击到的目标yaw位置 = 当前目标yaw位置 + 延迟时间
    st->vis_data.tar_yaw = st->vis_data.tar_yaw + st->vis_data.v_yaw * timeDelay - 0.0005f;

    // 计算四块装甲板的位置
    // 装甲板id顺序，以四块装甲板为例，逆时针编号
    //       2
    //    3     1
    //       0
    int use_1 = 1; // 使用前后距离还是左右距离
    int i = 0;
    int idx = 0; // 选择的装甲板
                 
    // 依次判断四块装甲板位置
    for (i = 0; i < 4; i++)
    {

        // 装甲板yaw位置 = 目标中心yaw位置 + 第i个装甲板的角度
        float tmp_yaw = st->vis_data.tar_yaw + i * PI / 2.0f;

        // 当前装甲板到目标中心距离 = 前后距离 或者 左右距离
        float r = use_1 ? st->vis_data.r1 : st->vis_data.r2;
        if (r < 0)
        {
            r = 0; // 或抛出错误
        }

        // 装甲板的位置 = 目标中心位置 +- 当前角度
        tar_position[i].x = st->vis_data.xw - r * cos(tmp_yaw);
        tar_position[i].y = st->vis_data.yw - r * sin(tmp_yaw);
        tar_position[i].z = use_1 ? st->vis_data.zw : st->vis_data.zw + st->vis_data.dz;

        // 当前装甲板角度
        tar_position[i].yaw = tmp_yaw;

        // 自动切换前后和左右距离
        use_1 = !use_1;
    }

    // 2种常见决策方案：
    // 1.计算枪管到目标装甲板yaw最小的那个装甲板
    // 2.计算距离最近的装甲板

    // 计算距离最近的装甲板
    //	float dis_diff_min = sqrt(tar_position[0].x * tar_position[0].x + tar_position[0].y * tar_position[0].y);
    //	int idx = 0;
    //	for (i = 1; i<4; i++)
    //	{
    //		float temp_dis_diff = sqrt(tar_position[i].x * tar_position[0].x + tar_position[i].y * tar_position[0].y);
    //		if (temp_dis_diff < dis_diff_min)
    //		{
    //			dis_diff_min = temp_dis_diff;
    //			idx = i;
    //		}
    //	}
    //

    // 计算枪管到目标装甲板yaw最小的那个装甲板

    // 目标装甲板最小的yaw角度 = 绝对值（当前自身的yaw角度 - 目标装甲板yaw角度）
    float yaw_diff_min = fabsf(*yaw - tar_position[0].yaw);
    for (i = 1; i < 4; i++)
    {

        // 求出每一块装甲板yaw角度
        float temp_yaw_diff = fabsf(*yaw - tar_position[i].yaw);

        // 求出最小的角度，并更新idx（决定打击的目标）
        if (temp_yaw_diff < yaw_diff_min)
        {
            yaw_diff_min = temp_yaw_diff;
            idx = i;
        }
    }

    // }

    // 设置打击位置 = 目标装甲板位置 + 目标当前各个方向速度 * 延迟时间
    *aim_z = tar_position[idx].z + st->vis_data.vzw * timeDelay;
    *aim_x = tar_position[idx].x + st->vis_data.vxw * timeDelay;
    *aim_y = tar_position[idx].y + st->vis_data.vyw * timeDelay;

    // 这里符号给错了（待修改）
    //  确保 sqrt 输入非负
    float distance = sqrt(fmaxf((*aim_x) * (*aim_x) + (*aim_y) * (*aim_y), 0));
    // 检查 atan2 输入
    if (fabsf(*aim_x) < 1e-6f && fabsf(*aim_y) < 1e-6f)
    {
        *yaw = 0; // 默认值
    }
    else
    {
        *yaw = atan2f(*aim_y, *aim_x);
    }
    // 给出pitch弹道修正
    float temp_pitch = -pitchTrajectoryCompensation(st, sqrt((*aim_x) * (*aim_x) + (*aim_y) * (*aim_y)) - st->s_bias,
                                                    *aim_z + st->z_bias, st->current_v);

    if (temp_pitch)
        *pitch = temp_pitch;

    if (*aim_x || *aim_y)
        *yaw = (float)(atan2(*aim_y, *aim_x));
    if (st->vis_data.id == 0)
    {
        *pitch = 0;
        *yaw = 0;
    }

    if(isnan(*yaw)) *yaw = 0;
}

// 从坐标轴正向看向原点，逆时针方向为正
